#include "AntiGyro.h"

namespace Horizon{
// 得到要跟踪的点
void AntiGyro::getCenter(){

    for(size_t k = 0;k < armors.size();k++){
        track_point[k].x = armors[k].center_.x + RADIUS*sin(abs(armors[k].h_yaw_angle_));
        track_point[k].z = armors[k].center_.z + RADIUS*cos(abs(armors[k].h_yaw_angle_));
        track_point[k].y = armors[k].center_.y;
    }
    

}

bool AntiGyro::isMoveGyro(){

    getCenter();
    float trackx[armors.size()];
    float trackz[armors.size()];

    for(size_t k = 0;k < armors.size();k++){
        trackx[k] = track_point[k].x;
        trackz[k] = track_point[k].z;
    }

    sort(trackx, trackx + armors.size());
    sort(trackz, trackz + armors.size());

    float vx = (trackx[armors.size()] - trackx[0])/FPS;
    float vz = (trackz[armors.size()] - trackz[0])/FPS;

    if(vx > MIN_SPEED || vz > MIN_SPEED){
        return true;
    }else{
        return false;
    }

}

GimbalPose AntiGyro::hitGyro(){

    bool flag_move_gyro;

    flag_move_gyro = isMoveGyro();

    if(flag_move_gyro){

        getCenter();
        float trackx[armors.size()];
        float trackz[armors.size()];

        for(size_t k = 0;k < armors.size();k++){
            trackx[k] = track_point[k].x;
            trackz[k] = track_point[k].z;
        }

        sort(trackx, trackx + armors.size());
        sort(trackz, trackz + armors.size());

        float vx = (trackx[armors.size()] - trackx[0])/FPS;
        float vz = (trackz[armors.size()] - trackz[0])/FPS;

        vector<Armor> list(armors);

        Armor armor_fix = smoothDataFilter1(list);
        
        armor_fix.center_.x += vx*(MINI_BATCH/FPS);
        armor_fix.center_.z += vz*(MINI_BATCH/FPS);

        float time = bullteFlyTime(armor_fix);

        start_time_ = armor_fix.h_time_stamp_ - time;

        frequence_ = CV_PI/(2*w_rad.rate);

    }else{
        vector<Armor> list(armors);

        Armor armor_fix = smoothDataFilter1(list);
        
        float time = bullteFlyTime(armor_fix);

        start_time_ = armor_fix.h_time_stamp_ - time;

        frequence_ = CV_PI/(2*w_rad.rate);

    }

}



}

